COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	(*$E+ Arithmetic routines used by AL *)
C00004 00003	(* external routines *)
C00005 00004	(* trig & scalar functions: vdot, vmagn *)
C00008 00005	(* vector functions: vmake, svmul, vsdiv, vadd, vsub, unitv, crossv, tvmul *)
C00012 00006	(* trans extraction routines: tpos, torient, taxis, tmagn *)
C00017 00007	(* trans functions: tmake, tvadd, tvsub, ttmul, tinvrt, vsaxwr, constr, vmkfrc *)
C00025 ENDMK
C⊗;
(*$E+ Arithmetic routines used by AL *)

program arith;

const pi = 3.1415926535; rad = 0.0174532925;

type scalar = real;
     vectorval = array [1..3] of real;
     vector = record  refcnt: integer; val: vectorval end;
     vectorp = ↑vector;
     transval = array [1..3,1..4] of real;
     trans = record  refcnt: integer; val: transval end;
     transp = ↑trans;

cstring = packed array [0..9] of ascii;
c5str = packed array [0..4] of ascii;
c20str = packed array [0..19] of ascii;

function newVector: vectorp; extern;
procedure relVector(v: vectorp); extern;
function newTrans: transp; extern;
procedure relTrans(t: transp); extern;

(* external routines *)

procedure ppLine; extern;				(* from EDIT.PAS *)
procedure ppOutNow; extern;
procedure ppChar(ch: ascii); extern;
procedure pp5(ch: c5str; length: integer); extern;
procedure pp10(ch: cstring; length: integer); extern;
procedure pp10L(ch: cstring; length: integer);extern;
procedure pp20(ch: c20str; length: integer); extern;
procedure pp20L(ch: c20str; length: integer); extern;
procedure ppReal(r: real); extern;

(* trig & scalar functions: vdot, vmagn *)

function sind(d: real): real; begin sind := sin(rad*d) end;
function cosd(d: real): real; begin cosd := cos(rad*d) end;
function tand(d: real): real; begin tand := sin(rad*d)/cos(rad*d) end;

function asin(x: real): real;
 begin
 if x = 1.0 then asin := 90.0
  else asin := arctan(x/sqrt(1.0-x*x))/rad;
 end;

function acos(x: real): real;
 var s: real;
 begin
 if x = 0.0 then acos := 90.0
  else
   begin
   s := arctan(sqrt(1.0-x*x)/x)/rad;
   if x < 0 then acos := 180.0 + s else acos := s;
   end;
 end;

function atan2(y,x: real): real;	(* 4-quadrant arctan(y/x) *)
 var ans: real;
 begin 
 if x = 0.0 then
   begin
   if y = 0.0 then ans := 0.0		(* Actually indeterminate, but ... *)
    else if y > 0.0 then ans := 90.0
    else ans := -90.0;
   end
  else
   begin 
   ans := arctan(y/x)/rad;
   if x < 0 then 
    if y < 0 then ans := ans - 180.0
     else ans := ans + 180.0;
   end;
 atan2 := ans;
 end;


function vdot (u,v: vectorp): scalar;
begin
 vdot := u↑.val[1]*v↑.val[1] + u↑.val[2]*v↑.val[2] + u↑.val[3]*v↑.val[3];
 if u↑.refcnt <= 0 then relVector(u);
 if v↑.refcnt <= 0 then relVector(v);
end;

function vmagn (v: vectorp): scalar;
begin
 with v↑ do vmagn := sqrt(val[1]*val[1] + val[2]*val[2] + val[3]*val[3]);
 if v↑.refcnt <= 0 then relVector(v);
end;

(* vector functions: vmake, svmul, vsdiv, vadd, vsub, unitv, crossv, tvmul *)

function vmake (a,b,c: scalar): vectorp;
var v: vectorp;
begin
 v := newVector;
 with v↑ do begin val[1] := a; val[2] := b; val[3] := c; end;
 vmake := v;
end;

function svmul (s: scalar; v: vectorp): vectorp;
var u: vectorp; i: 1..3;
begin
 if v↑.refcnt <= 0 then u := v else u := newVector;
 with v↑ do for i:= 1 to 3 do u↑.val[i] := s * val[i];
 svmul := u;
end;

function vsdiv (v: vectorp; s: scalar): vectorp;
var u: vectorp; i: 1..3;
begin
 if v↑.refcnt <= 0 then u := v else u := newVector;
 if s = 0 then
   begin
   pp20L('vsdiv: attempt to di',20); pp20('vide by zero!       ',13); ppLine;
   end;
 with v↑ do for i:= 1 to 3 do u↑.val[i] := val[i] / s;
 vsdiv := u;
end;

function vadd (u,v: vectorp): vectorp;
var w: vectorp; i: 1..3;
begin
 w := newVector;
 with w↑ do for i:= 1 to 3 do val[i] := u↑.val[i] + v↑.val[i];
 if u↑.refcnt <= 0 then relVector(u);
 if v↑.refcnt <= 0 then relVector(v);
 vadd := w;
end;

function vsub (u,v: vectorp): vectorp;
var w: vectorp; i: 1..3;
begin
 w := newVector;
 with w↑ do for i:= 1 to 3 do val[i] := u↑.val[i] - v↑.val[i];
 if u↑.refcnt <= 0 then relVector(u);
 if v↑.refcnt <= 0 then relVector(v);
 vsub := w;
end;

function unitv (v: vectorp): vectorp;
begin
 unitv := vsdiv(v,vmagn(v));
end;

function vcross (u,v: vectorp): vectorp;
var w: vectorp; i: 1..3;
begin
 w := newVector;
 with w↑ do
  begin
  val[1] := u↑.val[2]*v↑.val[3] - u↑.val[3]*v↑.val[2];
  val[2] := u↑.val[3]*v↑.val[1] - u↑.val[1]*v↑.val[3];
  val[3] := u↑.val[1]*v↑.val[2] - u↑.val[2]*v↑.val[1];
  end;
 if u↑.refcnt <= 0 then relVector(u);
 if v↑.refcnt <= 0 then relVector(v);
 vcross := w;
end;

function tvmul (t: transp; v: vectorp): vectorp;
var u: vectorp; i,j: 1..3;
begin
 u := newVector;
 with u↑ do
  for i:= 1 to 3 do
   begin
    val[i] := t↑.val[i,4];
    for j := 1 to 3 do val[i] := val[i] + t↑.val[i,j] * v↑.val[j];
   end;
 if t↑.refcnt <= 0 then relTrans(t);
 if v↑.refcnt <= 0 then relVector(v);
 tvmul := u;
end;

(* trans extraction routines: tpos, torient, taxis, tmagn *)

function tpos (t: transp): vectorp;
var v: vectorp; i: 1..3;
begin
 v := newVector;
 with t↑ do for i:= 1 to 3 do v↑.val[i] := val[i,4];
 if t↑.refcnt <= 0 then relTrans(t);
 tpos := v;
end;

function torient (t: transp): transp;
var r: transp; i,j: 1..3;
begin
 r := newTrans;
 with t↑ do
  for i := 1 to 3 do
   begin
   for j := 1 to 3 do r↑.val[i,j] := val[i,j];
   r↑.val[i,4] := 0;
   end;
 if t↑.refcnt <= 0 then relTrans(t);
 torient := r;
end;

function ssqrt(v: real): real;
 begin if (-0.000001 < v) and (v < 0) then ssqrt:= 0.0 else ssqrt:= sqrt(v) end;

function taxis (t: transp): vectorp;
 (* extracts the axis of rotation from a trans *)
var cx,cy,cz,a,b,c,d,cw: real;
begin
 a := t↑.val[1,1];
 b := t↑.val[2,2];
 c := t↑.val[3,3];
 cw := (a+b+c-1)/2;
 if cw > 0.9999 then taxis := vmake(0,0,1)	(* vector for nilrot = zhat *)
 else		(* *** use zhat ↑↑ in OMSI PASCAL version *** *)
  with t↑ do
   begin
    d := 3-a-b-c;
    cz := ssqrt((-a-b+c+1)/d);
    if cz < 0.001 then cy := ssqrt((-a+b-c+1)/d)
     else begin cy := (val[3,2] - val[1,2]*val[3,1] / (a-1)) * cz ;
		cy := cy / (1 - b + val[1,2]*val[2,1] / (a-1)); end;
    if cz+abs(cy) < 0.001 then cx := ssqrt((a-b-c+1)/d)
     else cx := -(val[2,1]*cy + val[3,1]*cz) / (a-1);
    taxis := vmake(cx,cy,cz);
   end;
 if t↑.refcnt <= 0 then relTrans(t);
end;

function tmagn (t: transp): scalar;
 (* finds the angle of rotation in a trans *)
var cx,cy,cz,a,b,c,d,cw,s: real;
begin
 with t↑ do
  begin
   a := val[1,1];
   b := val[2,2];
   c := val[3,3];
   cw := (a+b+c-1)/2;
   if cw > 0.9999 then tmagn := 0		(* angle for nilrot *)
    else
     begin
     d := 3-a-b-c;
     cz := ssqrt((-a-b+c+1)/d);
     if cz < 0.001 then cy := ssqrt((-a+b-c+1)/d)
      else begin cy := (val[3,2] - val[1,2]*val[3,1] / (a-1)) * cz ;
		 cy := cy / (1 - b + val[1,2]*val[2,1] / (a-1)); end;
     if cz+abs(cy) < 0.001 then cx := ssqrt((a-b-c+1)/d)
      else cx := -(val[2,1]*cy + val[3,1]*cz) / (a-1);
     if (-1.000001 < cw) and (cw < -1.0) then s := 180
      else if (1.0000 < cw) and (cw < 1.000001) then s := 0
      else s := acos(cw);
     if abs(cz) >= 0.577 then
	begin if (val[1,2]-val[2,1])/cz > 0 then s := -s end
     else if abs(cy) >= 0.577 then
	begin if (val[3,1]-val[1,3])/cy > 0 then s := -s end
     else if abs(cx) >= 0.577 then
	begin if (val[2,3]-val[3,2])/cx > 0 then s := -s end
     else
	begin
	pp20L('tmagn: rotation stra',20); pp10('ngeness!  ',8); ppLine;
	end;
     tmagn := s;
     end;
   if refcnt <= 0 then relTrans(t);
  end;
end;

(* trans functions: tmake, tvadd, tvsub, ttmul, tinvrt, vsaxwr, constr, vmkfrc *)

procedure tcopy(var tp,t: transval);	(* auxiliary routine *)
var i,j: 1..4;
begin for i := 1 to 3 do for j := 1 to 4 do tp[i,j] := t[i,j]; end;

function tmake (t: transp; v: vectorp): transp;
var tp: transp; i: 1..3;
begin
 if t↑.refcnt <= 0 then tp := t
  else begin tp := newTrans; tcopy(tp↑.val,t↑.val); end; (* copy rotation part *)
 with tp↑ do
  for i := 1 to 3 do val[i,4] := v↑.val[i];	(* and vector part *)
 if v↑.refcnt <= 0 then relVector(v);
 tmake := tp;
end;

function tvadd (t: transp; v: vectorp): transp;
var tp: transp; i,j: 1..3;
begin
 if t↑.refcnt <= 0 then tp := t
  else begin tp := newTrans; tcopy(tp↑.val,t↑.val); end; (* copy rotation part *)
 with tp↑ do
  for i := 1 to 3 do val[i,4] := val[i,4] + v↑.val[i];	(* add in vector *)
 if v↑.refcnt <= 0 then relVector(v);
 tvadd := tp;
end;

function tvsub (t: transp; v: vectorp): transp;
var tp: transp; i,j: 1..3;
begin
 if t↑.refcnt <= 0 then tp := t
  else begin tp := newTrans; tcopy(tp↑.val,t↑.val); end; (* copy rotation part *)
 with tp↑ do
  for i := 1 to 3 do val[i,4] := val[i,4] - v↑.val[i];	(* subtract vector *)
 if v↑.refcnt <= 0 then relVector(v);
 tvsub := tp;
end;

function ttmul (t1,t2: transp): transp;
var tp: transp; i,j,k: 1..4;
begin
 tp := newTrans;
 with tp↑ do
  for i := 1 to 3 do
   begin
   for j := 1 to 4 do			(* rotate t2 by orient(t1) *)
    begin
    val[i,j] := 0;
    for k := 1 to 3 do val[i,j] := val[i,j] + t1↑.val[i,k]*t2↑.val[k,j];
    end;
   val[i,4] := val[i,4] + t1↑.val[i,4];	(* add in t1 vector offset *)
   end;
 if t1↑.refcnt <= 0 then relTrans(t1);
 if t2↑.refcnt <= 0 then relTrans(t2);
 ttmul := tp;
end;

function tinvrt (t: transp): transp;
var tp: transp; i,j,k: 1..4;
begin	(* The result, (rot',trslat'), is defined: rot' = transpose(rot)
						trslat' = -(rot'*trslat) *)
 tp := newTrans;
 with tp↑ do
  for i := 1 to 3 do
   begin
   val[i,4] := 0;
   for j := 1 to 3 do
    begin
    val[i,j] := t↑.val[j,i];			(* transpose rotation part *)
    val[i,4] := val[i,4] - t↑.val[j,i] * t↑.val[j,4];	(* build up hβ0/9β[↔∨#?Iβ}3≠O↔"↓)$4R↓↓↓β.s⊃l4R↓↓β↔v!l4)εK→βRrsK↔≠≡sQ↓qj↓AβSF+9βK.bSKπw→#Q%Xh)βSNs[KQβQuβSβX4+↔v!l4(hS≠W;∨#'?9π3Oπc?⊃#πaRβ[↔∂&{KAmπ9iβK.1%iπ#Kπ;∨↓l4+6Iβ∂Bc∂e3∨Q3O]f≠]iβ⊗+π1mπ⊃iβS⊗;OAZβ%i↓
q9MlhS↔∨Nq↓!)πβK?∪.≠↔Mβ
βK?Sraβ∨'6+9βπFKMβ[.≠S?Iεa↓→εkπ∨;O#W∪∃π9↓)$hQβπaβQuβWvKSY#∂A%l4Rβ∂a↓Siβπbrs[π2[
ul4Rβ∂e↓Siβπbrs[π2[∩ul4Rβ∂i↓Siβπbrs[π2[~ul4RβO]↓SiβO'v!#]%Zβ∂]↓Siβ∂?≡!#]%Xh)βIβQuβ;/:SKπw→l4)π;'S!π∩yβ∪xh)↓β⊗+∨'8hQ↓β[∞bmE1
i↓iuε≠]↓-αAE7∂:I↓)β∨A↓)β∨Al4)αβ[π2[	1JuβQu↓!
k∂]%αQβ∂aαQβ∂eαiβ∂iαQβO]Xh)↓β62mEc~u↓ij↓!E7∨9%↓)ε≠a↓)ε≠i↓-ε≠e↓)π≠]l4R↓β[πeYI1Fj↓iu↓C	7∂]J↓)β∂B↓)β∂J↓-β∂R↓)βO;X4)↓π3π2m∩aJu↓Siβ∂]αY↓!En≠]%↓Rβ∂e↓Rβ∂elhQ↓β[∞bmI1≥i↓iuαAE7∂:I↓)β∨I↓)β∨Q↓5β∨A↓)β∨9l4)αβ[π2[→1FuβQu↓!
k∂]%αQβ∂aαQβ∂iαiβ∂eαQβO]Xh)↓β62mMc∩u↓ij↓!E7∨9%↓)ε≠e↓)ε≠i↓-ε≠a↓)π≠]l4R↓β[πeYM1Nj↓iuβ∨9↓-↓C	7∂]J↓)β∂R↓)β∂SX4)↓ε3?IβJ↓iu↓
βS=↓~β∪=β62o%c"u↓ij↓Al4R↓β↔;#X4)βN1βπbrsK↔≠≡sQ↓qj↓AβSF+9βK.b[↔∂&{I#πBIl4)π3Oπc?⊃↓iuπ⊃l4+.s⊃l4Ph+≠Wv≠S'?rβ∂?;∨#KW∂"C?K≥g3a3[GIiβ[.≠S?KαIiβS⊗;OAXh+[π∩βQiβ'∪π;OβYβa3Jciiβ6+∂S?↔↓mβ%R↓E99≠X4+.;'84Rβ?K≡rsK↔≠≡sQ↓ijβ?K≡rsK↔≠≡sQ↓-β	l%!RβO=β>)β∪?r;QβK.c↔πO*β?K≥π#?=β≡{?9↓RH4)βB↓iuβ.s'SYG3OW	G3a3?⊗9%%lHI!)β}YβS=π∪↔∂3∞K5β[Bβ;?]αQ$4)π3ce↓Siβ[O.⊃#[cJc?K≥KX$$%BQβ?-π#=βK.≠3π'jβ[ceεs?]↓RH4)βR↓iuβ.s'SYG3∂K?∨→#a37Ce%%Xh)βeβQuβ[∨∪?OMGQ3a%Xh)βQβQuβ;/:SKπw→l4)π;'S!π"yβ∪xh)↓β6{Iβ%βQu↓Eπ#=↓Mε#<4)α↓β↔>K84)α↓β[πe[%1Fj↓iuβEq;[πe[&ulhQ↓↓β62o%c∩u↓ijβfy;62o&kX4)↓αβ[π2↑I1NuβQuβjrs[π2↑Jul4R↓↓β[∞bo%1%i↓iuε{K≡yw3π2oMil4)α↓β↔;#X4)β␈∪≡y;⊗+≠∂;"↓iuβ␈∪≡y;⊗+≠∂;"↓5↓EXh)β'2β?K≡rsK↔≠≡sQ↓qj↓AβSF+9βK.b[↔∂&{I#?⊗9%l%BQβ∂πrβK↔∂f'5β␈∪≥β;␈9↓)$hQβK↔e3↔∂S␈⊃#a%ZβK↔26+∂S?∩Ce%mπ∪↔2[.≠S?IGQ%l%BQβ∪?v)β←'&AβS#/≠∃βS}y↓)$hQβ∂?w≠SKW∨!↓iuπ!l4+.s⊃l4Ph+≠Wv≠S'?rβ[7/7∪
#YRβ[↔∂&{KA%RβSKπw≠Al4Rβ[πIπ!iβS⊗;OAZβa3egQ3¬3∪QβK↔∞amβ%Rβ';S.;↔IlhQβ↔>K8%!RβSπ//→β≠?⊗≠∃β[.≠S?Iε;⊃βn/↔Mε	βSK∞sMβ←O#!βSF)βa7∂C'Mβ∞c?;≥π3↔∂S␈⊃↓)$hQβY↓SiβW;O#Y#YKX$$$JA)β7∞[∃β≠␈∪∂∃β6+
β'w#=βWvKQβ[.≠S?IαQ$4)π;'S!π2yβ∪xh)↓β⊗+∨'9πA↓iuπ3π2m
imβeβQuβ[∞bmJuZβi↓ijβ[π2[~umβ.s⊃l4RβK↔26+∂S?∩CY%lHH$%!Rβ∪?;*β←'SBβYβ;␈9↓)$hQβQ↓Siβ;↔=#Kπ;≠X4)β>KS!β%qβ∪<hQ↓β.;'84R↓β[πeYE1Fj↓iuβCX4)↓π3π2m
aJu↓SiβelhQ↓β[∞bmE1≥i↓iuπQl4)αβ'→↓GA↓u↓αIβπ;"↓#e↓j↓A%β&C↔84R↓↓↓β⊗+∨'8hQ↓↓↓ε3?IβJ↓iu↓
βS=↓~β∪=β⊗+∨'9π3π2m∩c&u↓Si↓Amπ3π2m~c&u↓Si↓Aβ.s⊃l4R↓↓↓β62mIc∩u↓ijβil4R↓↓↓β62mMc
u↓ij↓7ilhQ↓↓↓ε+;⊂4R↓↓β↔g≠∀4)α↓↓β.;'84R↓↓↓β∩↓iuβ∨KQ#BSa↓-πI+e%Xh)↓↓αβ¬↓ij↓5βeαyβ	lhQ↓↓↓ε⊃↓iuπA↓=β∪X4)↓α↓β[πeYI1Fj↓iuβX4)↓α↓β[πeYI1Jj↓iuβ∪X4)↓α↓β[πeYI1Nj↓iu↓βX4)↓α↓β[πeYM1Fj↓iu↓jβ	↓)πQl4)α↓↓β[∞bmM1∃i↓iuε	↓)βSX4)↓α↓β[πeYM1Nj↓iuβ∩↓)βaαiβ¬↓Rβel4R↓↓↓β.s⊃l4R↓β≠?∩β%↓ij↓EβSz↓Mβ∪zβ[π2↑I1RuβQu↓AXh)↓β.s⊃l4Rβ[7/7∪
↓ijβQl4Rβ↔;⊃Xh(4+⊗+∨'9ε+;⊃8hP4(